#include "kinectwinmgr.h"
#include "kinect.h"

#include <Windows.h>
#include <Ole2.h>
#include <mmsystem.h>
#include <cstdio>
#include <NuiApi.h>



#include <iostream>
namespace Tracking{
// Constructor
KinectWinMgr::KinectWinMgr(Kinect* p):
m_hNuiSkeletonEvent( NULL ),
m_hNuiProcessThread( NULL ),
//m_hNuiGuessGestureThread( NULL ),
m_hEvNuiProcessThreadStopEvent( NULL )
{
	_parent = p;
}

const vmml::mat4f& getRotationMatrixFromVectors(vec3f v1,vec3f v2){
		vmml::mat4f rotation = vmml::mat4f::IDENTITY;
		v1.normalize();
		v2.normalize();
		float dot = v1.dot(v2);
		if(! (dot!=dot)) //check for NaN
		{
			float angle = (float) acos(dot);
			if(! (angle!=angle))
			{
				vmml::vec3f cross = v1.cross(v2);
				cross.normalize();
				//std::cout<<angle<<std::endl;
				rotation.rotate(angle,cross);
				return rotation;
			
			}
		}
		return rotation;
}

// Destructor
KinectWinMgr::~KinectWinMgr()
{
	std::cout<<"kinect killed"<<std::endl;
}



// Initialize NUI runtime and start a thread to listen for events
bool KinectWinMgr::init()
{

    if( m_hNuiProcessThread || m_hEvNuiProcessThreadStopEvent || m_hNuiSkeletonEvent )
    {

        return true;
    }

    // Initialize NUI
    HRESULT hr = NuiInitialize( NUI_INITIALIZE_FLAG_USES_DEPTH | NUI_INITIALIZE_FLAG_USES_SKELETON );
    if( FAILED( hr ) )
    {

        return false;
    }

    // Enable skeleton tracking
    m_hNuiSkeletonEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
    hr =  NuiSkeletonTrackingEnable( m_hNuiSkeletonEvent, 0 );
    if( FAILED( hr ) )
    {

        return false;
    }

    // Start the NUI processing thread
    m_hEvNuiProcessThreadStopEvent = CreateEvent( NULL, FALSE, FALSE, NULL );
    m_hNuiProcessThread = CreateThread( NULL, 0, NuiProcessThread, this, 0, NULL );


	std::cout<<"Kinect initialized."<<std::endl;
    return true;
}

// Terminate the listening thread and uninitialize NUI runtime
void KinectWinMgr::uninit()
{

    
    // Disable skeleton tracking
    NuiSkeletonTrackingDisable();

    // Shutdown NUI
    NuiShutdown();

    // Stop the NUI processing thread
    if( m_hEvNuiProcessThreadStopEvent )
    {
        // Signal the thread and wait for it to terminate
        SetEvent( m_hEvNuiProcessThreadStopEvent );
        if( m_hNuiProcessThread )
        {
            WaitForSingleObject( m_hNuiProcessThread, INFINITE );
            CloseHandle( m_hNuiProcessThread );  
        }

        CloseHandle( m_hEvNuiProcessThreadStopEvent );
    }

    // Close skeleton tracking alert event handle
    if( m_hNuiSkeletonEvent )
    {
        CloseHandle( m_hNuiSkeletonEvent );
    }
    m_hNuiProcessThread = NULL;

    m_hEvNuiProcessThreadStopEvent = NULL;
    m_hNuiSkeletonEvent = NULL;

}

// Thread that processes events from NUI runtime
DWORD WINAPI KinectWinMgr::NuiProcessThread( LPVOID pParam )
{
    //KINECTWIN_FN_ENTRY;
    KinectWinMgr* pThis = (KinectWinMgr*)pParam;
    HANDLE hEvents[2];
    int nEventIdx = 0;

    // Configure events to be listened on
    hEvents[0] = pThis->m_hEvNuiProcessThreadStopEvent;
    hEvents[1] = pThis->m_hNuiSkeletonEvent;

    // Main thread loop
    while(1)
    {
        // Wait for an event to be signalled
        nEventIdx = WaitForMultipleObjects( sizeof(hEvents)/sizeof(hEvents[0]), hEvents, FALSE, 5 );
        if( 0 == ( WAIT_OBJECT_0 + nEventIdx ) )
        {
            // Exit from loop
            SetEvent( pThis->m_hEvNuiProcessThreadStopEvent );
            break;
        }
        else if( 1 == ( WAIT_OBJECT_0 + nEventIdx ) )
        {
            // Process skeleton motion alert
            pThis->NuiGotSkeletonAlert();
        }
        else if( WAIT_FAILED == nEventIdx )
        {

            break;
        }
    }
    //KINECTWIN_FN_EXIT;
    return 0;
}


void KinectWinMgr::NuiGotSkeletonAlert()
{
    // Get skeleton frame
    NUI_SKELETON_FRAME nuiSkeletonFrame = {0};
    HRESULT hr = NuiSkeletonGetNextFrame( 0, &nuiSkeletonFrame );
    if( FAILED( hr ) )
    {
      //  ReportError( "NuiSkeletonGetNextFrame", hr );
        return;
    }

    // Check if we have any skeleton
    int i = 0;
    bool bFoundSkeleton = false;
    for( i = 0; i < NUI_SKELETON_COUNT; i++ )
    {
        // Found atleast one tracked skeleton
        if( nuiSkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
        {
            bFoundSkeleton = true;
            break;
        }
    }
	Vector4 floor = nuiSkeletonFrame.vFloorClipPlane;
	
    if( bFoundSkeleton )
    {
		//m_skeleton = nuiSkeletonFrame;
        // Smooth the skeleton positions
        NUI_TRANSFORM_SMOOTH_PARAMETERS nuiSmoothParams;
        nuiSmoothParams.fCorrection = 0.3f;
        nuiSmoothParams.fJitterRadius = 1.0f;
        nuiSmoothParams.fMaxDeviationRadius = 0.5f;
        nuiSmoothParams.fPrediction = 0.4f;
        nuiSmoothParams.fSmoothing = 0.3f;
        //NuiTransformSmooth( &nuiSkeletonFrame, &nuiSmoothParams );

        // Convert left hand vector4 to x,y coordinates
        float fLeftX = 0, fLeftY = 0;
        Vector4 v4LeftHand = nuiSkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT];
		Vector4 v4RightHand = nuiSkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT];

  //      //NuiTransformSkeletonToDepthImage( v4LeftHand, &fLeftX, &fLeftY );

		////Vector4 floor = nuiSkeletonFrame.vFloorClipPlane;
		Vector4 vhead = nuiSkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HEAD];
		_parent->headPos.x() = vhead.x;_parent->headPos.y() = vhead.y;_parent->headPos.z() = vhead.z;
		//	
		//
		//vec3f lHand(v4LeftHand.x,v4LeftHand.y,v4LeftHand.z);
		//vec3f rHand(v4RightHand.x,v4RightHand.y,v4RightHand.z);
		//vec3f head(vhead.x,vhead.y,vhead.z);

		//vec3f headtoL;// = (vhead.x-v4LeftHand.x,vhead.y-v4LeftHand.y,vhead.z-v4LeftHand.z);
		//headtoL.x()= head.x()-lHand.x();
		//headtoL.y()= head.y()-lHand.y();
		//headtoL.z()= head.z()-lHand.z();
		//
		//vec3f headtoR;// = (vhead.x-v4LeftHand.x,vhead.y-v4LeftHand.y,vhead.z-v4LeftHand.z);
		//headtoR.x()= head.x()-rHand.x();
		//headtoR.y()= head.y()-rHand.y();
		//headtoR.z()= head.z()-rHand.z();

		//mat4f ro = getRotationMatrixFromVectors(headtoL,headtoR);
		////std::cout<<ro<<std::endl;

		//headPos.x() = head.x();headPos.y() = head.y();headPos.z() = head.z();

    }
}
}